#include "CharacterManager.h"

#include <ai/steering/SteeringPursue.h>
#include <graphic/Animation.h>
#include <graphic/AnimatedDrawable.h>
#include <graphic/DrawManager.h>
#include <graphic/Drawer.h>
#include <main/MainConstants.h>
#include <physic/Kinematic.h>

#define MAX_ENTITIES ( 20 )
#define ENEMY_SPEED ( 25.0f )

#define MIN_FRAMES_TO_REMOVE_CHILD (3.0 * MAX_FPS)

#define SQUARE_DIM ( 618 )
#define HALF_SQUARE_DIM (SQUARE_DIM / 2)

namespace {
    void limitMovement(float& x, float& z, const float width, const float height) {
        const float halfWidth = width / 2;
        const float halfHeight = height / 2;
        // Limit character movement 
        if (x  < (WINDOW_WIDTH / 2) - HALF_SQUARE_DIM + halfWidth){
            x = (WINDOW_WIDTH / 2) - HALF_SQUARE_DIM + halfWidth;
        } else if (x > (WINDOW_WIDTH / 2) + HALF_SQUARE_DIM - halfWidth) {
            x = (WINDOW_WIDTH / 2) + HALF_SQUARE_DIM - halfWidth;
        }

        if (z < (WINDOW_HEIGHT / 2) - HALF_SQUARE_DIM + halfHeight) {
            z = (WINDOW_HEIGHT / 2) - HALF_SQUARE_DIM + halfHeight;
        } else if (z > (WINDOW_HEIGHT / 2) + HALF_SQUARE_DIM - halfHeight) {
            z = (WINDOW_HEIGHT / 2) + HALF_SQUARE_DIM - halfHeight;
        }
    }
}

void CharacterManager::init() {
    mMinFramesToRemoveChild = MIN_FRAMES_TO_REMOVE_CHILD;

    mKinematics.reserve(MAX_ENTITIES);
    mInSteerings.reserve(MAX_ENTITIES);
    mOutSteerings.reserve(MAX_ENTITIES);
    mDrawables.reserve(MAX_ENTITIES);

    // Update global mouse position (used as target in steering)
    mMousePos.set(static_cast<float> (Globals::gMouseState.mX),
                  0.0f,
                  static_cast<float> (Globals::gMouseState.mY));

    // Register to update
    Globals::gGameLoop->updateMe(this);

    // Character kinematic
    Kinematic kinematic;
    float xPos = 120.0f;
    float zPos = 120.0f;
    kinematic.mPosition.set(xPos, 0.0f, zPos);
    kinematic.mOrientation = 0.0f;
    kinematic.mLinearVel.set(0.0f, 0.0f, 0.0f);
    mKinematics.push_back(kinematic);

    // Character steering input
    ArriveData arriveData;
    arriveData.mSrcPos = &mKinematics[0].mPosition;
    arriveData.mSrcLinearVel = &mKinematics[0].mLinearVel;
    arriveData.mTargetPos = &mMousePos;
    arriveData.mTargetRadius = 1.0f;
    arriveData.mSlowDownRadius = 100.0f;
    arriveData.mTimeToTarget = 0.25f;
    mInSteerings.push_back(arriveData);

    // Character steering output
    SteeringOutput steeringOutput;
    mOutSteerings.push_back(steeringOutput);

    // Character drawable
    Entity* entity = new Entity;
    entity->type = PLAYER;
    entity->staticObject = true;
    entity->pisycs = true;
    AnimatedDrawable* drawable = new AnimatedDrawable("mantis", entity, 84, 84);
	drawable->drawOnTop = true;
    drawable->setAnimationSpeed(0.08);
    drawable->updatePosition(mKinematics[0].mPosition.x,
                             mKinematics[0].mPosition.z,
                             mKinematics[0].mOrientation);
    Globals::gDrawManager->addDrawable(drawable);

    mDrawables.push_back(drawable);

    //
    // Initialize enemy
    //
    xPos = WINDOW_WIDTH / 2.0f;
    zPos = WINDOW_HEIGHT / 2.0f;
    mEnemyKinematic = new Kinematic();
    mEnemyKinematic->mPosition.set(xPos, 0.0f, zPos);
    mEnemyKinematic->mOrientation = 0.0f;
    mEnemyKinematic->mLinearVel.set(0.0f, 0.0f, 0.0f);

    // Enemy steering input
    mEnemyInSteering = new PursueData;
    mEnemyInSteering->mSrcPos = &mEnemyKinematic->mPosition;
    mEnemyInSteering->mSrcLinearVel = &mEnemyKinematic->mLinearVel;
    mEnemyInSteering->mTargetPos = &mKinematics[0].mPosition;
    mEnemyInSteering->mTargetLinearVel = &mKinematics[0].mLinearVel;
    mEnemyInSteering->mMaxPredictionTime = 0.5f;

    // Enemy steering output
    mEnemyOutSteering = new SteeringOutput();

    // Enemy drawable
    Entity* enemyEntity = new Entity;
    enemyEntity->type = ENEMY;
    enemyEntity->staticObject = false;
    enemyEntity->pisycs = true;
    mEnemyDrawable = new AnimatedDrawable("shark", enemyEntity, 63, 118);
    mEnemyDrawable->setAnimationSpeed(0.12);
    mEnemyDrawable->updatePosition(mEnemyKinematic->mPosition.x,
                                   mEnemyKinematic->mPosition.z,
                                   mEnemyKinematic->mOrientation);
	mEnemyDrawable->drawOnTop = true;
    Globals::gDrawManager->addDrawable(mEnemyDrawable);    
}

void CharacterManager::clear() {
    // Unregister from update
    Globals::gGameLoop->removeMe(this);
    assert(mKinematics.size() == mDrawables.size());
    assert(mDrawables.size() == mInSteerings.size());
    assert(mInSteerings.size() == mOutSteerings.size());

    mKinematics.clear();
    mInSteerings.clear();
    mOutSteerings.clear();
    mDrawables.clear();

    delete mEnemyKinematic;
    delete mEnemyInSteering;
    delete mEnemyOutSteering;
}

void CharacterManager::update() {
    if (mMinFramesToRemoveChild != 0) {
        --mMinFramesToRemoveChild;
    }

    // Update global mouse position (used as target in steering
    // for player
    mMousePos.set(static_cast<float> (Globals::gMouseState.mX),
                  0.0f,
                  static_cast<float> (Globals::gMouseState.mY));

    //
    // Update character and children
    //
    const size_t num = mKinematics.size();
    assert(num == mDrawables.size());
    assert(mDrawables.size() == mInSteerings.size());
    assert(mInSteerings.size() == mOutSteerings.size());
    const ArriveData * const arriveData = &mInSteerings[0];
    SteeringOutput *outSteerings = &mOutSteerings[0];
    Kinematic* kinematic = &mKinematics[0];
    arrive(arriveData, outSteerings, num);
    integrate(kinematic, outSteerings, num);

    for (size_t i = 0; i < num; ++i) {
		if(Globals::gDrawManager->isRegistered(mDrawables[i])) {
            const float width = mDrawables[i]->texData().mRect->w;
            const float height = mDrawables[i]->texData().mRect->h;
            limitMovement(mKinematics[i].mPosition.x,
                          mKinematics[i].mPosition.z,
                          width,
                          height);

            mDrawables[i]->updatePosition(mKinematics[i].mPosition.x,
                                          mKinematics[i].mPosition.z,
                                          mKinematics[i].mOrientation);
		}
    }

    //
    // Update enemy
    //
    pursue(mEnemyInSteering, mEnemyOutSteering, 1);
    const float maxSpeed = ENEMY_SPEED;
    integrate(mEnemyKinematic, mEnemyOutSteering, 1, &maxSpeed);

    const float width = mEnemyDrawable->texData().mRect->w;
    const float height = mEnemyDrawable->texData().mRect->h;
    limitMovement(mEnemyKinematic->mPosition.x,
                  mEnemyKinematic->mPosition.z,
                  width,
                  height);

    mEnemyDrawable->updatePosition(mEnemyKinematic->mPosition.x,
                                   mEnemyKinematic->mPosition.z,
                                   mEnemyKinematic->mOrientation);
}

void CharacterManager::addChild(AnimatedDrawable* drawable) {
    assert(drawable);
    {
        const size_t num = mKinematics.size();
		std::cout << "Num children = " << num - 1 << std::endl;
        assert(num == mDrawables.size());
        assert(mDrawables.size() == mInSteerings.size());
        assert(mInSteerings.size() == mOutSteerings.size());
    }

    // kinematic
    Kinematic kinematic;
    const float xPos = mDrawables[mDrawables.size() - 1]->getPosition().mCenterPosX;
    const float zPos = mDrawables[mDrawables.size() - 1]->getPosition().mCenterPosY;
    kinematic.mPosition.set(xPos, 0.0f, zPos);
    kinematic.mOrientation = mDrawables[mDrawables.size() - 1]->getPosition().mOrientation;
    kinematic.mLinearVel.set(0.0f, 0.0f, 0.0f);
    mKinematics.push_back(kinematic);

    // steering input
    ArriveData arriveData;
    arriveData.mSrcPos = &mKinematics[mKinematics.size() - 1].mPosition;
    arriveData.mSrcLinearVel = &mKinematics[mKinematics.size() - 1].mLinearVel;
    arriveData.mTargetPos = &mKinematics[mKinematics.size() - 2].mPosition;
    arriveData.mTargetRadius = 1.0f;
    arriveData.mSlowDownRadius = 100.0f;
    arriveData.mTimeToTarget = .25f;
    mInSteerings.push_back(arriveData);

    // steering output
    SteeringOutput steeringOutput;
    mOutSteerings.push_back(steeringOutput);

    // drawable
	drawable->drawOnTop = true;
    mDrawables.push_back(drawable);
}

// Returns true if a child could be removed.
// It is useful to know if there were
// remaining children because
// otherwise, character should die
bool CharacterManager::removeChild() {
    if (mMinFramesToRemoveChild > 0) {
        return true;
    }

    const size_t num = mKinematics.size();
    assert(num == mDrawables.size());
    assert(mDrawables.size() == mInSteerings.size());
    assert(mInSteerings.size() == mOutSteerings.size());

    if (num == 1) {
        return false;
    }
    AnimatedDrawable* drawable = mDrawables[num - 1];
    Globals::gDrawManager->removeDrawable(drawable);
    mDrawables.pop_back();

    mKinematics.pop_back();
    mInSteerings.pop_back();
    mOutSteerings.pop_back();

    mMinFramesToRemoveChild = MIN_FRAMES_TO_REMOVE_CHILD;

    return true;
}